
#ifndef ODOMETRY_TRICYCLE_H_
#define ODOMETRY_TRICYCLE_H_

#include <boost/accumulators/accumulators.hpp>
#include <boost/accumulators/statistics/stats.hpp>
#include <boost/accumulators/statistics/rolling_mean.hpp>
#include <functional>
#include "chassis_controller/odometry.h"

namespace chassis_controller
{ 
  /**
   * \brief The Odometry class handles odometry readings
   * (2D pose and velocity with related timestamp)
   */
  class TricycleOdometry : public Odometry
  {
  public:
    explicit TricycleOdometry(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node);
    /// @brief 第一次计算初始化为当前值
    /// @param time 
    /// @param pivot_value 各驱动单元的实际值
    void init(const rclcpp::Time &time, std::vector<Spivot_value> &pivot_value) ;

    /// @brief 使用位置反馈计算车体中心位置
    /// @param pivot_value  各驱动单元的实际值
    /// @param pivot_param  各驱动单元的参数
    /// @param time 
    /// @return 正确计算
    void update(std::vector<Spivot_value> &pivot_value, std::vector<Spivot_param> &pivot_param , const rclcpp::Time &time, bool standstill,double odom_factor) ;

    /// @brief 使用速度积分计算车体中心位置
    /// @param pivot_value 各驱动单元的实际值
    /// @param pivot_param 各驱动单元的参数
    /// @param time 
    //void updateOpenLoop(std::vector<Spivot_value> &pivot_value, std::vector<Spivot_param> &pivot_param, const rclcpp::Time &time) ;


    /**
     * \brief heading getter
     * \return heading [rad]
     */
    double getHeading() 
    {
      return position_.a;
    }

    /**
     * \brief x position getter
     * \return x position [m]
     */
    double getX() 
    {
      return position_.x;
    }

    /**
     * \brief y position getter
     * \return y position [m]
     */
    double getY() 
    {
      return position_.y;
    }

    /**
     * \brief linear velocity getter
     * \return linear velocity [m/s]
     */
    double getVx() 
    {
      return velocity_.x;
    }
    /**
     * \brief linear velocity getter
     * \return linear velocity [m/s]
     */
    double getVy() 
    {
      return velocity_.y;
    }
    /**
     * \brief angular velocity getter
     * \return angular velocity [rad/s]
     */
    double getVa() 
    {
      return velocity_.a;
    }

  private:
  
    double normAngle2(double angle)
    {
        double fRad = angle;
        // Scale the angle into [-PI, +)
        while(fRad < -PI)
            fRad += 2*PI;
        // Scale the angle into [-PI, PI)
        while (fRad >= PI)
            fRad -= 2*PI;
        return fRad;
    }


    
    /**
     * \brief Integrates the velocities (linear and angular) using 2nd order Runge-Kutta
     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
     */
    void integrateRungeKutta2(double linear, double angular);

    /**
     * \brief Integrates the velocities (linear and angular) using exact method
     * \param linear  Linear  velocity   [m] (linear  displacement, i.e. m/s * dt) computed by encoders
     * \param angular Angular velocity [rad] (angular displacement, i.e. m/s * dt) computed by encoders
     */
    void integrateExact(double linear, double angular);

    Sxya frameTrans(Sxya original , double angle);

    /// Current timestamp:
    rclcpp::Time timestamp_;

    /// Current pose in map:
    Sxya pos_vehicle_;    

    /// Current pose in map:
    Sxya position_; 

    /// Previous pose:
    Sxya previous_pos_; 

    /// Current velocity:
    Sxya velocity_;  // [m/s]

    /// Previous velocity:
    Sxya previous_vel_;  // [m/s]

    //
    Sxya pivot_vel_; 

    //   
    double pivot_pos_; 

    //   
    double previous_pivot_pos_; 

    Sxya delta_pivot_pos_;
    Sxya delta_pos_in_vehicle_;
    Sxya pivot_vel_in_vehicle_;
     
    /// Previous wheel position/state [rad]:


};
}

#endif /* ODOMETRY_H_ */
